#include <iostream>

#include "predictor_acceleration.h"
#include "predictor_link.h"

using namespace std;

Predictor_const_a::Predictor_const_a(std::string id) :
  Predictor_base(id)
{
  _string_type = "Acceleration";

  add_input_link(new Predictor_link( PARAMETER )); // acceleration
  add_input_link(new Predictor_link( INPUT )); // velocity
  add_input_link(new Predictor_link( INPUT )); // position
  _predicted_inputs.resize(_input_links.size());

  _complexity = 3.0;
}

Predictor_const_a::~Predictor_const_a()
{
}

void Predictor_const_a::save_state(float time)
{
  save_input(time);
}

void Predictor_const_a::predict(float time)
{
  float dt = time - _saved_time;
  _predicted_inputs[0] = _input_links[0]->get_value(); // input acceleration
  _predicted_inputs[1] = _saved_inputs[1] + _input_links[0]->get_value() * dt; // input velocity
  // predict new position
  _predicted_inputs[2] = _saved_inputs[2] + _input_links[1]->get_value() * dt
      + 0.5 * _input_links[0]->get_value() * dt * dt;
}

void Predictor_const_a::activate_output_link(void)
{
  if (_active) {
    _output_link->activate();
  }
  else {
    //cout << "  " << _string_id << ":  De-activating output " << endl;
    _output_link->deactivate();
  }
}
